#include "simulation/client/client.h"

#include <atomic>
#include <mutex>
#include <utility>

#include "simulation/client/detail/cxx.h"

namespace simulation {
namespace client {

struct ClientInner {
  void SetClient(rust::Box<ffi::ClientWraper>&& client) {
    this->client = new rust::Box<ffi::ClientWraper>(std::move(client));
  }

  rust::Box<ffi::ClientWraper> TakeClient() {
    auto r = std::move(*client);
    ClearClient();
    return r;
  }

  void ClearClient() {
    delete client;
    client = nullptr;
  }

  void SetVCH(rust::Box<ffi::VehicleControlHandleWrapper>&& vch) {
    this->vch = new rust::Box<ffi::VehicleControlHandleWrapper>(std::move(vch));
  }

  void ClearVCH() {
    delete vch;
    vch = nullptr;
  }

  void SetSH(rust::Box<ffi::SensorHandleWrapper>&& sh) {
    this->sh = new rust::Box<ffi::SensorHandleWrapper>(std::move(sh));
  }

  void ClearSH() {
    delete sh;
    sh = nullptr;
  }

  void SetSSH(rust::Box<ffi::StopSensorHandleWrapper>&& sh) {
    this->ssh = new rust::Box<ffi::StopSensorHandleWrapper>(std::move(sh));
  }

  void ClearSSH() {
    delete ssh;
    ssh = nullptr;
  }

  std::atomic_bool stop = ATOMIC_VAR_INIT(false);

  unsigned int timeout_ms;

  rust::Box<ffi::ClientWraper>* client = nullptr;
  rust::Box<ffi::SensorHandleWrapper>* sh = nullptr;
  rust::Box<ffi::StopSensorHandleWrapper>* ssh = nullptr;

  std::mutex vch_mtx;
  rust::Box<ffi::VehicleControlHandleWrapper>* vch = nullptr;
};

Client::Client() { inner_ = new ClientInner(); }

Client::Client(Client&& other) {
  inner_ = other.inner_;
  other.inner_ = new ClientInner();
}

Client::~Client() {
  auto inner = reinterpret_cast<ClientInner*>(inner_);

  inner->ClearSH();
  inner->ClearVCH();
  inner->ClearClient();

  delete inner;
}

Client& Client::operator=(Client&& other) {
  inner_ = other.inner_;
  other.inner_ = new ClientInner();

  return *this;
}

std::tuple<bool, std::string> Client::Connect(const std::string& uri,
                                              unsigned int timeout_ms) {
  auto inner = reinterpret_cast<ClientInner*>(inner_);

  inner->ClearClient();

  try {
    inner->SetClient(ffi::connect(uri, timeout_ms));
    inner->timeout_ms = timeout_ms;

    return std::make_tuple(true, std::string());
  } catch (rust::Error& e) {
    return std::make_tuple(false, std::string(e.what()));
  }
}

std::tuple<bool, std::string> Client::Run(const std::string& client_id,
                                          const std::string& vehicle_type,
                                          double x, double y, double z,
                                          double qw, double qx, double qy,
                                          double qz,
                                          const Callbacks& callbacks) {
  auto inner = reinterpret_cast<ClientInner*>(inner_);

  inner->ClearSSH();
  inner->ClearSH();
  inner->ClearVCH();

  if (inner->client) {
    auto client = inner->TakeClient();
    try {
      auto h =
          ffi::create_vehicle(std::move(client), client_id, vehicle_type,
                              {x, y, z}, {qw, qx, qy, qz}, inner_->timeout_ms);
      inner->SetVCH(h->get_vehicle_control_handle());
      inner->SetSH(h->get_sensor_handle());
      inner->SetSSH(h->get_stop_sensor_handle());

      return Run(callbacks);
    } catch (rust::Error& e) {
      inner->stop.store(false);
      return std::make_tuple(false, std::string(e.what()));
    }
  } else {
    inner->stop.store(false);
    return std::make_tuple(false, std::string("Uninitialized"));
  }
}

std::tuple<bool, std::string> Client::Run(const std::string& client_id,
                                          const std::string& token,
                                          const Callbacks& callbacks) {
  auto inner = reinterpret_cast<ClientInner*>(inner_);

  inner->ClearSSH();
  inner->ClearSH();
  inner->ClearVCH();

  if (inner->client) {
    auto client = inner->TakeClient();
    try {
      auto h = ffi::get_vehicle(std::move(client), client_id, token,
                                inner_->timeout_ms);
      inner->SetVCH(h->get_vehicle_control_handle());
      inner->SetSH(h->get_sensor_handle());
      inner->SetSSH(h->get_stop_sensor_handle());

      return Run(callbacks);
    } catch (rust::Error& e) {
      inner->stop.store(false);
      return std::make_tuple(false, std::string(e.what()));
    }
  } else {
    inner->stop.store(false);
    return std::make_tuple(false, std::string("Uninitialized"));
  }
}

std::tuple<bool, std::string> Client::SendControlCommand(
    const VehicleControl& control) {
  auto inner = reinterpret_cast<ClientInner*>(inner_);

  if (inner->vch) {
    auto& vch = *inner->vch;
    try {
      std::unique_lock<decltype(inner_->vch_mtx)> lk(inner_->vch_mtx);

      vch->set_control(control, inner_->timeout_ms);
      return std::make_tuple(true, std::string());
    } catch (rust::Error& e) {
      return std::make_tuple(false, std::string(e.what()));
    }
  } else {
    return std::make_tuple(false, std::string("Uninitialized"));
  }
}

std::tuple<bool, std::string> Client::SetVehicleLocalization(
    double pos_x, double pos_y, double pos_z, double ori_qw, double ori_qx,
    double ori_qy, double ori_qz) {
  auto inner = reinterpret_cast<ClientInner*>(inner_);

  if (inner_->vch) {
    auto& vch = *inner->vch;
    try {
      std::unique_lock<decltype(inner_->vch_mtx)> lk(inner_->vch_mtx);

      vch->set_transform({pos_x, pos_y, pos_z},
                         {ori_qw, ori_qx, ori_qy, ori_qz});
      return std::make_tuple(true, std::string());
    } catch (rust::Error& e) {
      return std::make_tuple(false, std::string(e.what()));
    }
  } else {
    return std::make_tuple(false, std::string("Uninitialized"));
  }
}

void Client::Stop() {
  auto inner = reinterpret_cast<ClientInner*>(inner_);

  inner->stop.store(true);
  inner->ClearSSH();
}

std::tuple<bool, std::string> Client::Run(const Callbacks& callbacks) {
  auto inner = reinterpret_cast<ClientInner*>(inner_);

  try {
    while (!inner->stop.load()) {
      auto& sh = *inner->sh;

      auto sensors = sh->get_sensors();

      if (sensors.test_result.size()) {
        callbacks.on_test_result(sensors.test_result[0]);
        break;
      }

      if (sensors.localization.size()) {
        callbacks.on_localization(sensors.localization[0]);
      }

      if (sensors.perception.size()) {
        callbacks.on_perception(sensors.perception[0]);
      }

      if (sensors.chassis.size()) {
        callbacks.on_chassis(sensors.chassis[0]);
      }

      if (sensors.collision_event.size()) {
        callbacks.on_collision_event(sensors.collision_event[0]);
      }

      if (sensors.lane_invasion_event.size()) {
        callbacks.on_lane_invasion_event(sensors.lane_invasion_event[0]);
      }

      if (sensors.camera_images.size()) {
        std::vector<Image> images(sensors.camera_images.size());
        for (decltype(images.size()) i = 0; i < images.size(); ++i) {
          images[i] = std::move(sensors.camera_images[i]);
        }
      }
    }

    inner->stop.store(false);
    return std::make_tuple(true, std::string());
  } catch (rust::Error& e) {
    inner->stop.store(false);
    return std::make_tuple(false, std::string(e.what()));
  }
}

}  // namespace client
}  // namespace simulation